import matplotlib.pyplot as plt
import numpy as np

class IMCPID:
    def __init__(self, K=1.0, tau=50.0, theta=5.0, lam=40.0, dt=1.0):
        """
        IMC-PID控制器参数
        :param K: 系统增益
        :param tau: 系统时间常数
        :param theta: 系统时延
        :param lam: IMC滤波参数
        :param dt: 控制周期
        """
        # 计算 IMC-PID 参数
        self.Kp = (2 * tau + theta) / (K * (2 * lam + theta))
        self.Ti = tau + theta / 2
        self.Td = (tau * theta) / (2 * tau + theta)
        self.dt = dt

        # 控制器状态
        self.integral = 0.0
        self.prev_error = 0.0
        self.prev_measurement = 0.0

    def compute(self, setpoint, measurement):
        error = setpoint - measurement

        # 积分项 - 带抗饱和和积分限幅
        self.integral += error * self.dt
        self.integral = np.clip(self.integral, -5.0, 5.0)  # 更严格的积分限幅

        # 微分项 - 使用测量值微分减少设定值突变的影响
        derivative = (self.prev_measurement - measurement) / self.dt

        # IMC-PID 输出
        output = (self.Kp * error + 
                 (self.Kp / self.Ti) * self.integral - 
                 self.Kp * self.Td * derivative)
        
        self.prev_error = error
        self.prev_measurement = measurement
        return output


class HVACController:
    def __init__(self, season="winter"):
        self.season = season
        self.pid = IMCPID(K=0.8, tau=60.0, theta=5.0, lam=40.0, dt=1.0)  # 调整参数
        
        # 控制参数
        self.min_on_time = 300  # 最小开启时间 (秒) - 缩短以更快响应
        self.min_off_time = 300  # 最小关闭时间 (秒)
        self.hysteresis = 0.3  # 减小迟滞带 (°C)
        self.emergency_shutdown_threshold = 2.0  # 紧急关闭阈值(°C)
        
        # 状态变量
        self.last_switch_time = 0
        self.current_state = "OFF"
        self.time_elapsed = 0

    def control(self, setTemp, roomTemp):
        pid_output = self.pid.compute(setTemp, roomTemp)
        self.time_elapsed += 1
        
        # 紧急关闭检查 - 当温度过高时强制关闭
        if roomTemp > setTemp + self.emergency_shutdown_threshold:
            self.current_state = "OFF"
            self.last_switch_time = self.time_elapsed
            return "HEATER OFF (EMERGENCY)", pid_output
        
        # 根据季节确定控制方向
        if self.season == "winter":
            temp_diff = setTemp - roomTemp
            need_heat = temp_diff > self.hysteresis
            stop_heat = temp_diff < -self.hysteresis
        
        # 检查最小运行/停机时间
        time_since_switch = self.time_elapsed - self.last_switch_time
        
        if self.current_state == "ON":
            if stop_heat and time_since_switch >= self.min_on_time:
                self.current_state = "OFF"
                self.last_switch_time = self.time_elapsed
        else:
            if need_heat and time_since_switch >= self.min_off_time:
                self.current_state = "ON"
                self.last_switch_time = self.time_elapsed
        
        return f"HEATER {self.current_state}", pid_output


# ========== 修正后的房屋热模型 ==========
class HouseModel:
    def __init__(self, initial_temp=18.0):
        self.temp = initial_temp
        self.outdoor_temp = 10.0  # 冬季室外温度
        self.heat_loss_coeff = 0.01 # 减小热损失系数
        self.heating_power = 0.5  # 增大加热功率
        self.thermal_mass = 100.0  # 热质量
        
    def update(self, heating_on):
        # 热损失 - 与室内外温差成正比
        temp_diff = self.temp - self.outdoor_temp
        heat_loss = self.heat_loss_coeff * temp_diff
        
        # 加热效果 - 仅与HVAC开关状态有关
        heat_gain = self.heating_power if heating_on else 0.0
        
        # 温度变化考虑热质量
        # 修正公式：加热增加温度，热损失降低温度
        delta_temp = (heat_gain - heat_loss) / self.thermal_mass
        self.temp += delta_temp
        
        # 添加微小随机扰动
        self.temp += np.random.normal(0, 0.005)
        
        return self.temp


# ========== 仿真 ==========
controller = HVACController(season="winter")
house = HouseModel(initial_temp=18.0)

setTemp = 22.0    # 目标温度

temps = []
actions = []
outputs = []
steps = 10000  # 仿真时间步长

for t in range(steps):
    action, pid_output = controller.control(setTemp, house.temp)
    roomTemp = house.update("ON" in action)
    
    temps.append(roomTemp)
    actions.append(1 if "ON" in action else 0)
    outputs.append(pid_output)

# ========== 绘图 ==========
plt.figure(figsize=(14, 10))

# 温度曲线
plt.subplot(3, 1, 1)
plt.plot(range(steps), temps, label="Room Temperature (°C)", color="tab:blue")
plt.axhline(setTemp, color="tab:red", linestyle="--", label="Set Temperature")
plt.axhline(setTemp + controller.hysteresis, color="tab:orange", linestyle=":", label="Hysteresis Band")
plt.axhline(setTemp - controller.hysteresis, color="tab:orange", linestyle=":")
plt.axhline(setTemp + controller.emergency_shutdown_threshold, 
            color="tab:red", linestyle="-.", label="Emergency Shutdown")
plt.ylabel("Temperature (°C)")
plt.title("Corrected HVAC Control with Proper Thermal Dynamics")
plt.legend(loc="upper right")
plt.grid(True)

# PID输出
plt.subplot(3, 1, 2)
plt.plot(range(steps), outputs, label="PID Output", color="tab:purple")
plt.axhline(0, color="black", linestyle="--")
plt.ylabel("PID Output")
plt.legend(loc="upper right")
plt.grid(True)

# 设备状态
plt.subplot(3, 1, 3)
plt.step(range(steps), actions, label="Heater State (1=ON,0=OFF)", 
         color="tab:green", where="post")
plt.xlabel("Time step")
plt.ylabel("Control Signal")
plt.legend(loc="upper right")
plt.grid(True)

plt.tight_layout()
plt.show()
